/*
 * @Author: _oufen
 * @Date: 2023-07-11 20:23:19
 * @LastEditTime: 2023-08-03 01:54:39
 * @Description:  主要控制代码
 */
/**
 *************************************************************************************************************************
 * @file    control.c
 * @author  oufen
 * @version V1.0
 * @date    2023-07-10
 * @brief   Control模块.c文件配置
 *************************************************************************************************************************
 * @attention
 *
 *
 *************************************************************************************************************************
 */

/* Includes -------------------------------------------------------------------------------------------------------------*/
#include "control.h"

/* 定义 -----------------------------------------------------------------------------------------------------------------*/
Param_InitTypedef Param;
Flag_InitTypedef Flag;

void Limit(int16_t Servo_X_Pwm, int16_t Servo_Y_Pwm)
{
    if (Param.Servo_X_PWM > 2000)
    {
        Param.Servo_X_PWM = 2000;
    }
    else if (Param.Servo_X_PWM < 1000)
    {
        Param.Servo_X_PWM = 1000;
    }

    if (Param.Servo_Y_PWM > 2500)
    {
        Param.Servo_Y_PWM = 2500;
    }
    else if (Param.Servo_Y_PWM < 1800)
    {
        Param.Servo_Y_PWM = 1800;
    }
}

uint8_t My_time(uint32_t myTime, uint16_t num)
{
    if (myTime < num) // 中心点等待2s
    {
        myTime++;
    }
    else
    {
        myTime = 0;
        return 1;
    }
    return 0;
}

/**
 * @brief 主控制函数  基础部分第二问
 * @param  无
 * @retval  无
 **/
void mainControl(void)
{
    static int flag = -1;       // 步骤运行
    static uint32_t myTime = 0; // 计时

    switch (flag)
    {
    case -1:
        if (myTime < 200) // 中心点等待2s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag = 0;
        }
        break;
    case 0:
        TIM4->CCR1 += 1; // 到达原点最上方点  *1   *1点坐标为 (1460,772)
        if (TIM4->CCR1 >= 772)
        {
            TIM4->CCR1 = 772;
        }
        if (myTime < 200) // 中心点等待2s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag = 1;
        }
        break;
    case 1:              // 慢慢移动  至最左上方点
        TIM4->CCR2 += 1; // 到达最左上方点  +1 (1600,772)
        if (TIM4->CCR2 >= 1600)
        {
            TIM4->CCR2 = 1600;
        }
        if (myTime < 300) // 中心点等待2s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag = 2;
        }
        break;

    case 2:
        TIM4->CCR2 -= 1; // 到达原点最上方点  *1   *1点坐标为 (1460,772)
        if (TIM4->CCR2 <= 1460)
        {
            TIM4->CCR2 = 1460;
        }

        TIM4->CCR1 -= 1; // 到达原点最上方点  *1   *1点坐标为 (1460,772)
        if (TIM4->CCR1 <= 755)
        {
            TIM4->CCR1 = 755;
        }
        if (myTime < 300) // 中心点等待2s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag = 3;
        }
        break;
    case 3:
        TIM4->CCR2 -= 1; // 到达原点最上方点  +2  +2(1320,772)
        if (TIM4->CCR2 <= 1320)
        {
            TIM4->CCR2 = 1320;
        }

        if (myTime < 300) // 中心点等待2s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag = 4;
        }
        break;
    case 4:
        TIM4->CCR1 -= 1; // 到达原点最右方点  *3 *3点坐标为 (1320,632)
        if (TIM4->CCR1 <= 632)
        {
            TIM4->CCR1 = 632;
        }
        if (myTime < 300) // 中心点等待2s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag = 5;
        }
        break;
    case 5:
        TIM4->CCR1 -= 1; // 到达原点最右方点 +3  +3(1320,492)
        if (TIM4->CCR1 <= 492)
        {
            TIM4->CCR1 = 492;
        }
        if (myTime < 300) // 中心点等待2s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag = 7;
        }
        break;
    // case 6:
    //     TIM4->CCR2 += 1; // 到达原点最右方点 +3  +3(1320,492)
    //     if (TIM4->CCR2 >= 1460)
    //     {
    //         TIM4->CCR2 = 1460;
    //     }
    //     if (myTime < 300) // 中心点等待2s
    //     {
    //         myTime++;
    //     }
    //     else
    //     {
    //         myTime = 0;
    //         flag = 7;
    //     }
    //     break;
    case 7:
        TIM4->CCR2 += 1; // 到达原点最右方点 *4 *4点坐标为 (1460,492)
        if (TIM4->CCR2 >= 1460)
        {
            TIM4->CCR2 = 1460;
        }
        if (myTime < 300) // 中心点等待2s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag = 8;
        }
        break;
    case 8:
        TIM4->CCR2 += 1; // 到达原点最右方点 +4    +4(1600,492)

        if (TIM4->CCR2 >= 1600)
        {
            TIM4->CCR2 = 1600;
        }
        if (myTime < 300) // 中心点等待2s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag = 9;
        }
        break;
    case 9:
        TIM4->CCR1 += 1; // 到达原点最右方点 *2  *2点坐标为 (1600,632)

        if (TIM4->CCR1 >= 632)
        {
            TIM4->CCR1 = 632;
        }
        if (myTime < 300) // 中心点等待2s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag = 10;
        }
        break;
    case 10:
        TIM4->CCR1 += 1; // 到达原点最右方点 +1   +1 (1600,772)
        if (TIM4->CCR1 >= 772)
        {
            TIM4->CCR1 = 772;
        }

        // TIM4->CCR2 -= 1; // 到达原点最右方点 +4    +4(1600,492)

        // if (TIM4->CCR2 < 1580)
        // {
        //     TIM4->CCR2 = 1580;
        // }

        if (myTime < 300) // 中心点等待2s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag = 11;
        }
        break;
    case 11:
        if (myTime < 300) // 中心点等待2s  蜂鸣器 响3s
        {
            myTime++;
            Buzzer_IN = 1;
        }
        else
        {
            Buzzer_IN = 0;
            myTime = 0;
            flag = 12;
        }
    case 12:
        break;
    default:
        break;
    }
}

void my_test(void)
{
    static int flag1 = -1;      // 步骤运行
    static uint32_t myTime = 0; // 计时

    switch (flag1)
    {
    case -1:
        if (myTime < 100) // 中心点等待2s  蜂鸣器 响3s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag1 = 0;
        }
        break;
    case 0:
        TIM_CCxCmd(TIM4, TIM_Channel_1, TIM_CCx_Disable);
        TIM_CCxCmd(TIM4, TIM_Channel_2, TIM_CCx_Disable);
    default:
        break;
    }
}

void my_test1(void)
{
    static int flag2 = -1;      // 步骤运行
    static uint32_t myTime = 0; // 计时
    switch (flag2)
    {
    case -1:
        if (myTime < 200) // 中心点等待2s  蜂鸣器 响3s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag2 = 0;
        }
        break;

    case 0:
        if (x_pwm > x_middle)
        {
            x_pwm -= 2;
            if (x_pwm <= x_middle)
            {
                // x_pwm = x_middle;
                //  TIM_CCxCmd(TIM4, TIM_Channel_1, TIM_CCx_Disable);
                TIM_CCxCmd(TIM4, TIM_Channel_2, TIM_CCx_Disable);
            }
        }
        else if (x_pwm < x_middle)
        {
            x_pwm += 2;
            if (x_pwm >= x_middle)
            {
                // x_pwm = x_middle;
                TIM_CCxCmd(TIM4, TIM_Channel_2, TIM_CCx_Disable);
            }
        }

        if (y_pwm > y_middle)
        {
            y_pwm -= 2;
            if (y_pwm <= y_middle)
            {
                // y_pwm = y_middle;
                TIM_CCxCmd(TIM4, TIM_Channel_1, TIM_CCx_Disable);
            }
        }
        else if (y_pwm < y_middle)
        {
            y_pwm += 2;
            if (y_pwm >= y_middle)
            {
                // y_pwm = y_middle;
                TIM_CCxCmd(TIM4, TIM_Channel_1, TIM_CCx_Disable);
            }
        }
        // if (myTime < 100) // 中心点等待2s  蜂鸣器 响3s
        // {
        //     myTime++;
        // }
        // else
        // {
        //     myTime = 0;
        //     flag2 = 0;
        // }
        break;
    case 1:
        if (myTime < 100) // 中心点等待2s  蜂鸣器 响3s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag2 = 0;
        }
        break;
    default:
        break;
    }
}

void my_test2(void)
{
    static int flag3 = -1;      // 步骤运行
    static uint32_t myTime = 0; // 计时

    switch (flag3)
    {
    case -1:
        x_pwm -= 1;
        if (x_pwm <= 600)
        {
            x_pwm = 600;
        }
        if (myTime < 100) // 中心点等待1s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag3 = 0;
        }
        break;
    case 0:
        x_pwm -= 1;
        if (x_pwm <= 440)
        {
            x_pwm = 440;
        }
        if (myTime < 100) // 中心点等待1s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag3 = 1;
        }
        break;
    case 1:
        y_pwm += 1;
        if (y_pwm >= 1460)
        {
            y_pwm = 1460;
        }
        if (myTime < 100) // 中心点等待1s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag3 = 2;
        }
        break;
    case 2:
        y_pwm += 1;
        if (y_pwm >= 1570)
        {
            y_pwm = 1570;
        }
        if (myTime < 100) // 中心点等待1s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag3 = 3;
        }
        break;
    case 3:
        x_pwm += 1;
        if (x_pwm >= 582)
        {
            x_pwm = 582;
        }
        if (myTime < 100) // 中心点等待1s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag3 = 4;
        }
        break;
    case 4:
        x_pwm += 1;
        if (x_pwm >= 715)
        {
            x_pwm = 715;
        }
        if (myTime < 100) // 中心点等待1s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag3 = 5;
        }
        break;
    case 5:
        y_pwm -= 1;
        if (y_pwm <= 1460)
        {
            y_pwm = 1460;
        }
        if (myTime < 100) // 中心点等待1s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag3 = 6;
        }
        break;
    case 6:
        y_pwm -= 1;
        if (y_pwm <= 1300)
        {
            y_pwm = 1300;
        }
        if (myTime < 100) // 中心点等待1s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag3 = 7;
        }
        break;
    case 7:
        if (myTime < 100) // 中心点等待1s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            TIM_CCxCmd(TIM4, TIM_Channel_1, TIM_CCx_Disable);
            TIM_CCxCmd(TIM4, TIM_Channel_2, TIM_CCx_Disable);
        }
    default:
        break;
    }
}

void my_test3(void)
{
    static int flag4 = -1;      // 步骤运行
    static uint32_t myTime = 0; // 计时

    switch (flag4)
    {
    case -1:
        x_pwm -= 1;
        if (x_pwm <= 600)
        {
            x_pwm = 600;
        }
        if (myTime < 100) // 中心点等待1s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag4 = 0;
        }
        break;
    case 0:
        x_pwm -= 1;
        if (x_pwm <= 465)
        {
            x_pwm = 465;
        }
        if (myTime < 100) // 中心点等待1s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag4 = 1;
        }
        break;
    case 1:
        y_pwm += 1;
        if (y_pwm >= 1460)
        {
            y_pwm = 1460;
        }
        if (myTime < 100) // 中心点等待1s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag4 = 2;
        }
        break;
    case 2:
        y_pwm += 1;
        if (y_pwm >= 1575)
        {
            y_pwm = 1575;
        }
        if (myTime < 100) // 中心点等待1s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag4 = 3;
        }
        break;
    case 3:
        x_pwm += 1;
        if (x_pwm >= 600)
        {
            x_pwm = 600;
        }
        if (myTime < 100) // 中心点等待1s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag4 = 4;
        }
        break;
    case 4:
        x_pwm += 1;
        if (x_pwm >= 740)
        {
            x_pwm = 740;
        }
        if (myTime < 100) // 中心点等待1s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag4 = 5;
        }
        break;
    case 5:
        y_pwm -= 1;
        if (y_pwm <= 1430)
        {
            y_pwm = 1430;
        }
        if (myTime < 100) // 中心点等待1s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag4 = 6;
        }
        break;
    case 6:
        y_pwm -= 1;
        if (y_pwm <= 1310)
        {
            y_pwm = 1310;
        }
        if (myTime < 100) // 中心点等待1s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            flag4 = 7;
        }
        break;
    case 7:
        if (myTime < 100) // 中心点等待1s
        {
            myTime++;
        }
        else
        {
            myTime = 0;
            TIM_CCxCmd(TIM4, TIM_Channel_1, TIM_CCx_Disable);
            TIM_CCxCmd(TIM4, TIM_Channel_2, TIM_CCx_Disable);
        }
    default:
        break;
    }
}

/**
 * @brief 	TIM6中断   10ms
 * @param 	无
 * @retval  无
 */
float speed1,
    speed2;

float X_pwm,
    Y_pwm;
void TIM6_IRQHandler(void) // 10ms
{
    if (TIM_GetITStatus(TIM6, TIM_IT_Update) != RESET)
    {
        LED_RED = !LED_RED; // 测试程序是否卡死
        TIM_ClearITPendingBit(TIM6, TIM_IT_Update);

        // mainControl();

        // my_test();

        // my_test1();
        // my_test2();
        my_test3();

        // 中心点坐标为  160 120
        // X轴方向
        //        if (((OpenMv_data1 << 8) | OpenMv_data2) != 0)
        //        {
        //            Param.Servo_X_PWM -= Servo_X_pid_control(); // 这里的+还是-  要根据摄像头的摆放情况来修改   由于摄像头是倒着放的

        //            if (Param.Servo_X_PWM > 1800)
        //            {
        //                Param.Servo_X_PWM = 1800;
        //            }
        //            else if (Param.Servo_X_PWM < 1000)
        //            {
        //                Param.Servo_X_PWM = 1000;
        //            }
        //        }
        //        else
        //        {
        //            pid_servo_X.err = 0;
        //            pid_servo_X.integral = 0;
        //        }
        //        // y轴方向
        //        if (OpenMv_data3 != 0)
        //        {
        //            Param.Servo_Y_PWM -= Servo_Y_pid_control();
        //            if (Param.Servo_Y_PWM > 600)
        //            {
        //                Param.Servo_Y_PWM = 600;
        //            }
        //            else if (Param.Servo_Y_PWM < 400)
        //            {
        //                Param.Servo_Y_PWM = 400;
        //            }
        //        }
        //        else
        //        {
        //            pid_servo_Y.err = 0;
        //            pid_servo_Y.integral = 0;
        //        }

        //        TIM4->CCR1 = Param.Servo_Y_PWM;
        //        TIM4->CCR2 = Param.Servo_X_PWM;
    }
}
/*****************************************************END OF FILE*********************************************************/
